Robot Localization and 3d Mapping: Observability Analysis and Applications

In this work we investigate a quaternion-based formulation of 3D Simultaneous Localization and Mapping with Extended Kalman Filter (EKF-SLAM) using relative pose measurements. The equations of the filter do not rely on heuristic solutions for preserving the unit norm of quaternions of rotation, nor introduce spurious measurements to force the unitary norm constraint. The proposed model is formally shown to be completely observable using standard tools for observability analysis in piecewise linear systems. Moreover, we report numerical results of the application of the proposed approach on real data from the Rawseeds dataset. The contribution is motivated by the possibility of abstracting multi-sensorial information in terms of relative pose measurements and for its straightforward extensions to the multi robot case.